/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 * @file CAHistogramECorrelationScanMatch.h
 * 
 * This is a laser scan matching algorithm, i.e. it calculates
 * translation (x,y) and rotation (angle) between two laser scans.
 * 
 * The algorithm is divided into two quite separate parts that 
 * are combined to produce the final match (x, y, angle):
 * Part 1) Angle correction algorithm: based on laser scans' angle 
 *         histograms.
 * Part 2) Translation correction algorithm: based on iterative 
 *         evidence grid correlation.
 *  
 * These algorithms are described in more details in the
 * CAHistogramECorrelationScanMatch.cpp
 *
 * @author Seppo Heikkil� * @acknoledgements Jari Saarinen
 * 
 * References:
 * [1] Heikkil�S., Development of Laser Based Localisation Methods for 
 *     Personal Navigation System - PeNa, Master's thesis, Helsinki
 *     University of Technology, March, 2005, Espoo, Finland.
 *
 */

#ifndef C_ANGLE_HISTOGRAM_EVIDENCEGRID_CORRELATION_SCAN_MATCH_H_
#define C_ANGLE_HISTOGRAM_EVIDENCEGRID_CORRELATION_SCAN_MATCH_H_    
    
#include <math.h>
#include "CScanMatchInterface.h"
    
/**
 * Struct needed to compare to dimensional ints.
 */
typedef struct __intpoint_t {
        int x;
        int y;
}intpoint_t;

/**
 * Struct needed so that angles and correlation can be sorted
 * more easily.
 */
typedef struct __angleAndCorrelation {
        float angle;
        float correlation;
}angleAndCorrelation_t;

class CAHistogramECorrelationScanMatch : public CScanMatchInterface{
    private:

      /**
       * Stepsize for the angle search. In radians.
       * 0.01 radians is about 0.5 degrees.
       */
      float angle_resolution;

      // relation of circle arc to its radius (nature's coefficient)
      float pi; 

      /**
       * Position search area (in meters)
       */
      float searcharea;

      /** 
       * Algorithm angle search area (in radians)
       */
      float anglesearchdist;

      /**
       * Maximum number of discrete angles used in small angle correlation.
       * For example with resolution of 0.01 radians, this would be PI/0.01 = 314.
       */
      static const int SMALL_ANGLE_DISCRETE_ANGLES_MAX = 700;

      /** 
       * Maximum number of small angle maximums that can be saved and returned.
       */
      static const int SMALL_ANGLE_SUMARRAYINDEX_MAX = 200;
      
      ///////////////////////////////////////////////////////////
      ///////////////////////////////////////////////////////////
      
    protected:
    public:
	/**
	* Default constructor
	* Assumes: 361 measurement with 0.5 deg... 
	*/
	CAHistogramECorrelationScanMatch();
	
	/**
	 * Initializes the laser scan matcher with custom search area and
	 * seach angle.
	 *
	 * @param searchArea size of xy-correlation algorithm search area
	 *                   (meters). Has to be dividable by 0.04.
	 * @param angleSearchArea distance for the angle to be searched (radians).
	 */
	CAHistogramECorrelationScanMatch(float searchArea, 
									   float angleSearchArea);
        
        ~CAHistogramECorrelationScanMatch();
        
        /////////////////////////////////////////////
        //// SCAN MATCH INTERFACE ///////////////////
        ////////////////////////////////////////////
        /**
         * Updates the position estimate with the new measurement @meas. 
         * @param &meas The latest range scan measurement
         * @param &est  The estimated position, where the measurement is from.
         * @param Goodness The goodness (probability/likelihood) of the match [0...1] 1 is perfect 0 is none 
         */
        float update(ScanMatch::scan &meas, ScanMatch::pose est);
	

////////////////////////////////////////////////////////////////////////////////////
//// CAHistogramECorrelationScanMatch - CANNOT BE USED FROM SCANMATCH INTERFACE ////
////////////////////////////////////////////////////////////////////////////////////


/**
 * Match the two given scans. Returns how much first scan(act) has to
 * be shifted for it to be aligned with last one(ref).
 * 
 * @param refxo Reference scan x-coordinates.
 * @param refyo Reference scan y-coordinates.
 * @param refxyN Reference scan array sizes.
 * @param actxo Reference scan x-coordinates.
 * @param actyo Reference scan y-coordinates.
 * @param actxyN Reference scan array sizes.
 * @param itialpose initial guess for the correction.
 * @param retCorrection returned correction between scans.
 * @param correlation xy correlation for the match.
 * @param S location covariace matrix.
 * @param anglevar angle variance is returned here.
 * @param searcharea size of xy-correlation algorithm search area.
 * @param anglesearchdist distance for the angle to be searched, in radians
 *
 * @return 0 if all ok, else -1
 */
int matchScans(float *refxo, float *refyo, int refxyN,
					 float *actxo, float *actyo, int actxyN,
					 float *initialpose, float *retCorrection, 
					 float *correlation, float **S, float *anglevar,
					 float searcharea, float anglesearchdist);
		

/**
 * Corrects the angle between two scans by using angles between 
 * scan points. 
 * 
 * @param x1 x-coordinates of first laserscan data.
 * @param y1 y-coordinates of first laserscan data.
 * @param size1 size of x1 and y1 buffers, has to be > step.
 * @param x2 x-coordinates of second laserscan data.
 * @param y2 x-coordinates of second laserscan data.
 * @param size2 size of x2 and y2 buffers, has to be > step.
 * @param searchdist distance to search for answer
 * @param discrete resolution for the search, in radians
 * @param initguess initial estimate for the correction angle
 * @param anglebuffer returned angle corrections in radians.
 * @param corrbuffer correlation sums for returned angle correction in radians.
 *        Gives some hint how believable the returned angle was. If NULL parameter,
 *        then sums are not returned.
 * @param anglebuffersize size of anglebuffer and corrbuffer, this many angles 
 *        is returned at maximum.
 * @param var angle variance.
 *
 * @return number of local angle maximums found, if failed -1
 */
int estimateAngle(float *x1, float *y1, int size1, 
                              float *x2, float *y2, int size2, 
                              float searchdist, float discrete, 
                                  float initguess,
                                  float *anglebuffer, float *corrbuffer, int anglebuffersize,
					     float *var);

/**
 * Corrects the angle between two scans by using angles between 
 * scan points. Used points are separated with step number of points.
 * Correction is calculated using "histogram correlation matching".
 * 
 * @param x1 x-coordinates of first laserscan data.
 * @param y1 y-coordinates of first laserscan data.
 * @param size1 size of x1 and y1 buffers, has to be > step.
 * @param x2 x-coordinates of second laserscan data.
 * @param y2 x-coordinates of second laserscan data.
 * @param size2 size of x2 and y2 buffers, has to be > step.
 * @param searchdist distance to search for answer
 * @param discrete resolution for the search, in radians
 * @param initguess initial estimate for the correction angle
 * @param step number of points within one angle approximation
 * @param anglebuffer returned angle corrections in radians.
 * @param corrbuffer correlation sums for returned angle correction in radians.
 *                Gives some hint how believable the returned angle was. If NULL parameter,
 *        then sums are not returned.
 * @param anglebuffersize size of anglebuffer and corrbuffer, this many angles 
 *        is tried to be returned.
 * @param var variance to be returned.
 * @param mean angle mean value to be returned.
 *
 * @return number of local angle maximums found, if failed -1
 */
int smallAngleCorrection(float *x1, float *y1, int size1, 
                                     float *x2, float *y2, int size2, 
                                     float searchdist, float discrete, 
                                                 float initguess, 
                                                 int step, 
                                                 float *anglebuffer, float *corrbuffer, int anglebuffersize,
						   float *var, float *mean);


/**
 * Rotates and translates the given x,y-points with pos. The angle correction
 * is done first, then the required translation.
 *
 * @param pos translation x = pos(1), y = pos(2). Rotation angle = pos(3).
 * @param xpoints x-points of scan
 * @param ypoints y-points of scan
 * @return 0 if all ok, else failed
 */
int rotateAndTranslate(float pos[3], float *xpoints, 
						 float *ypoints, int points_size);


/**
 * Corrects translation error with "grid matching". Given scan coordinates
 * are discreted in the area defined by min and max. Then coordinates
 * will be sorted according to x-value using qsort. Then for each point in
 * first scan, a nearest point from second scan is searched. The nearest point 
 * is estimated to be hit(== 0) or miss(==1), or something between those two.
 * The better the scan correlation is, the smaller the correlation sum is.
 * 
 * @param x1 x-coordinates of first data.
 * @param y1 y-coordinates of first data.
 * @param size1 size of x1 and y1 buffers, has to be > 0 but <= 361.
 * @param x2 x-coordinates of second data.
 * @param y2 x-coordinates of second data.
 * @param size2 size of x2 and y2 buffers, has to be > 0 but <= 361.
 * @param deltax2 change to be subtracted from x2 coordinates.
 * @param deltay2 change to be subtracted from y2 coordinates.
 * @param searchdist distance to search for answer.
 * @param stepsize resolution for the search.
 * @param dx correction in x-direction.
 * @param dy correction in y-direction.
 * @param best_correlation correlation with dx and dy.
 * @param minx minimum x-coordinate for search.
 * @param maxx maximum x-coordinate for search.
 * @param miny minimum y-coordinate for search.
 * @param maxy maximum y-coordinate for search.
 * @param get_probability_mask 1 if mask is wanted, else 0.
 * @param S covariance matrix, size 2x2
 * @return 0 if all ok, else -1
 */
int gridMatch(float *x1, float *y1, int size1, 
                          float *x2, float *y2, int size2, 
                          float deltax2, float deltay2,
                          float searchdist, float stepsize,
                          float *dx, float *dy, float *best_correlation,
                          int minx, int maxx, int miny, int maxy,
					int get_probability_mask, float **S);


/**
 * Sorts the given angle estimates and removes duplicated 
 * angles. Number of found suitable angles is returned.
 * 
 * @param anglebuffer array containing angles, returned angles 
 *        are also save in here.
 * @param correlation values are saved here
 * @param size size of anglebuffer array.
 * @param meanangle mean angle value
 * @param var variance for angle
 * @param resolution defines the distance within which angles are considered as one
 * @return number of angles, -1 if failed
*/
int arrayAngleEstimation(float *anglebuffer, float *corbuffer, int size,
						   float *meanangle, float *var, float resolution);
 

// FIXME, add comments to the interface
int findClosestPoint(intpoint_t *indexes1, int indexes1_size,
                                         intpoint_t *indexes2, int indexes2_size,
                                         int x, int y, int res,
                                         int get_probability_mask, 
					       float *best_correlation, float *corr);

/**
 * Estimates the given probability mask with ellipse.
 *
 * @param mask place probability mask, small value is more probable.
 * @param msize width and height of the mask.
 * @param S covariance matrix, size is 2x2
 * @return 0 if estimation valid, 1 if not valid, -1 if failed
 */
 int paraboloidEstimation(float **mask, int msize, float **S);

/**
 * Multiply probability matrix with right size position matrix.
 *
 * @param probability normalized matrix with probabilities. Size is psize x psize.
 * @param psize probability matrix size.
 * @param est parameter estimations returned.
 * @return 0 if all ok, else -1.
 */
 int multiplyHx(float **probability, int psize, float *est);

/**
 * Converts given distance to x-y coordinates
 *
 * @param R distance array, size is 361, if distance < 0 then
 * it is ignored.
 * @param x x-coordinate
 * @param y y-coordinate
 * @return number of coordinates, or -1 if fails
 */
int distance2xycoordinates(float *R, float *x, float *y);


/**
 * Converts given distance to x-y coordinates
 * Improved algorithm. Uses the new interface and is able to deal with arbitrary 
 * sized scans 
 * @param ScanMatch::scan scan, if distance < 0 then
 * it is ignored.
 * @param x x-coordinate
 * @param y y-coordinate
 * @return number of coordinates, or -1 if fails
 */
int distance2xycoordinates(ScanMatch::scan &m, float *x, float *y);
/**
 * Integrates the given differential Pose into original
 * pose. 
 * 
 * @param original Original Pose, size is 3
 * @param differential Differential change which is to be 
 *        integrated to the pose. Size is also 3(x, y, angle).
 * @param result Integrated pose is saved here.
 * @return 0 if all ok, else -1
 */
int integrate(float *original, float *differential, float *result);

};

#endif
